
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_imu_invensense.h
  * @author     baiyang
  * @date       2021-10-21
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include "sensor_imu.h"
#include "sensor_imu_backend.h"

#include <common/time/gp_time.h>
#include <common/gp_rotations.h>
#include <device_manager/dev_mgr.h>
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
enum Invensense_Type {
    Invensense_MPU6000=0,
    Invensense_MPU6500,
    Invensense_MPU9250,
    Invensense_ICM20608,
    Invensense_ICM20602,
    Invensense_ICM20601,
    Invensense_ICM20789,
    Invensense_ICM20689,
};

typedef struct {
    sensor_imu_backend backend;

    // acclerometers on Invensense sensors will return values up to
    // 24G, but they are not guaranteed to be remotely linear past
    // 16G
    uint16_t multiplier_accel; // INT16_MAX/(26*GRAVITY_MSS);

    int16_t _raw_temp;

    // instance numbers of accel and gyro data
    uint8_t _gyro_instance;
    uint8_t _accel_instance;

    float temp_sensitivity; // 1.0f/340, degC/LSB
    float temp_zero;        // 36.53f, degC
    
    float _temp_filtered;
    float _accel_scale;
    float _gyro_scale;

    float _fifo_accel_scale;
    float _fifo_gyro_scale;
    LowPassFilt _temp_filter;
    uint32_t last_reset_ms;
    uint8_t reset_count;

    enum RotationEnum _rotation;

    // enable checking of unexpected resets of offsets
    bool _enable_offset_checking;

    // ICM-20602 y offset register. See usage for explanation
    uint8_t _saved_y_ofs_high;

    gp_device_t _dev;

    // which sensor type this is
    enum Invensense_Type _mpu_type;

    // are we doing more than 1kHz sampling?
    bool _fast_sampling;

    // what downsampling rate are we using from the FIFO for gyros?
    uint8_t _gyro_fifo_downsample_rate;

    // what downsampling rate are we using from the FIFO for accels?
    uint8_t _accel_fifo_downsample_rate;

    // ratio of raw gyro to accel sample rate
    uint8_t _gyro_to_accel_sample_ratio;

    // what rate are we generating samples into the backend for gyros?
    uint16_t _gyro_backend_rate_hz;

    // what rate are we generating samples into the backend for accels?
    uint16_t _accel_backend_rate_hz;

    // Last status from register user control
    uint8_t _last_stat_user_ctrl;    

    // buffer for fifo read
    uint8_t *_fifo_buffer;

    /*
      accumulators for sensor_rate sampling
      See description in _accumulate_sensor_rate_sampling()
    */
    struct {
        Vector3f_t accel;
        Vector3f_t gyro;
        uint8_t accel_count;
        uint8_t gyro_count;
        LowPassFilt_vec3f accel_filter; // 默认值，采样频率，4000 截至频率，188
    } _accum;
} sensor_imu_invensense;

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void sensor_imu_invensense_ctor(sensor_imu_invensense *invensense, sensor_imu *imu, gp_device_t dev, enum RotationEnum rotation);

sensor_imu_backend *sensor_imu_invensense_probe(sensor_imu *imu, gp_device_t dev, enum RotationEnum rotation);

/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



